%% 例题5-9：针对平面2R机器人，可设定轨迹类型、电机工作模式、速度前馈、加速度前馈和集中力矩前馈的独立关节PID控制器仿真例程
% 根据题义，假定两个关节电机和系统参数均相同的情况，因此，非线性力矩计算公式不具有通用性
% 利用循环模拟控制系统时钟，假定每个采样周期都进行伺服控制，迭代计算电机速度，且每次伺服控制都更新伺服期望值定时器
%% 程序启动，清理内存和绘图
clc;
clear;
clf;

%% 定义和设定控制相关变量
%% 定义跟踪轨迹曲线类型符号
Hold_on = 0;
Traj_Slope = 1;
Traj_Pos_S = 2;
Traj_Vol_S = 3;
%% 选择跟踪轨迹
% Traj_Type = Hold_on;
% Traj_Type = Traj_Slope;
% Traj_Type = Traj_Pos_S;
Traj_Type = Traj_Vol_S;

%% 定义电机工作模式符号
Amp_Voltage = 0;
Amp_Current = 1;
%% 选择电机工作模式
% Amp_Type = Amp_Voltage;
Amp_Type = Amp_Current;

%% 选择前馈
Vol_Forward = 1; % 速度前馈：1-有效，0-无效；
Acc_Forward = 1; % 加速度前馈：1-有效，0-无效；
Tq_Forward = 1; % 非线性力矩集中前馈：1-有效，0-无效；

%% 设定减速比
N = 21; 

%% 设定闭环系统响应特性
ts = 0.1;%调节时间
Zeta = 1;%阻尼比
if (Zeta == 1), Omega_n = 4.75/ts; end % 临界阻尼系统的自然频率
if (Zeta < 1), Omega_n = 3.5/(Zeta*ts); end % 欠阻尼系统的自然频率
if (Zeta > 1), Omega_n = 3.3/((Zeta- sqrt(Zeta^2-1))*ts); end % 过阻尼系统的自然频率

%% 设定模型偏差
D_m = 0.05; % 理论质量与实际质量的偏差率
D_L = 0.05; % 理论杆长与实际杆长的偏差率

%% 初始化
%% 电机参数初始化
Ra = 2.49;
La = 6.1e-4;
Ka = 8.22e-2;
Ke = 8.24e-2;

%% 系统参数初始化
%% 单杆参数
g = 9.8;
m = 0.5; % 实际质量
L = 0.1; % 实际杆长
Br = 4.1e-4;
Ir = 1.19e-5;
Bl = 2e-2;
m_D = m * (1 - D_m); % 理论质量
L_D = L * (1 - D_L); % 理论杆长

%% 平面2R机器人驱动空间平均惯量矩阵
% 理论值
M11_Average_D = (3*m_D*L_D^2)/N^2 + Ir; % 1#关节电机平均等效转动惯量
M22_Average_D = (m_D*L_D^2)/N^2 + Ir; % 2#关节电机平均等效转动惯量
Mm_Average_D = [M11_Average_D 0;0 M22_Average_D]; % 机器人驱动空间平均惯量矩阵
% 实际值
M11_Average = (3*m*L^2)/N^2 + Ir; % 1#关节电机平均等效转动惯量
M22_Average = (m*L^2)/N^2 + Ir; % 2#关节电机平均等效转动惯量
Mm_Average = [M11_Average 0;0 M22_Average]; % 机器人驱动空间平均惯量矩阵
%% 平面2R机器人驱动空间等效阻尼矩阵
Bm11 = Bl/N^2 + Br; % 1#关节电机平均等效转动惯量
Bm22 = Bl/N^2 + Br; % 2#关节电机平均等效转动惯量
Bm = [Bm11 0; 0 Bm22]; % 机器人驱动空间等效阻尼矩阵

%% 计算电机控制模型参数
%% 速度模式电机模型参数
Ku = 3;
Tmv = (Ra .* Mm_Average_D) .* inv((Ke*Ka) .* eye(2) + Ra .* Bm);
Kmv = (Ka*Ku) .* inv((Ke*Ka) .* eye(2) + Ra .* Bm);
Kdv = Kmv * Ra ./(Ka*Ku);

%% 力矩模式电机模型参数
Kg = 1;
Kt = Ka*Kg;
Tmt = Mm_Average_D * inv(Bm);
Kmt = Kt .* inv(Bm);
Kdt = inv(Bm);

%% 通用电机模型参数
if(Amp_Type == Amp_Voltage) 
    Tm = Tmv;
    Km = Kmv;
    Kd = Kdv;
end
if(Amp_Type == Amp_Current) 
    Tm = Tmt;
    Km = Kmt;
    Kd = Kdt;
end

%% 计算控制器增益
%% 机器人PID控制器增益矩阵
Tv = Tm;
Kv = (2*Zeta*Omega_n) .* inv(Km);
Kp = [Omega_n/(2*Zeta) 0; 0 Omega_n/(2*Zeta)];
%% 速度和加速度前馈增益矩阵
Kvff = inv(Km);
Kaff = Tm .* inv(Km);
%% 力矩前馈增益矩阵
if (Amp_Type == Amp_Voltage), Ktff = Kd .* inv(Km); end % 速度模式电机的力矩前馈增益
if (Amp_Type == Amp_Current), Ktff = (1/Kt) .* eye(2); end % 力矩模式电机的力矩前馈增益

%% 设置仿真时间、步长和步数
dt = 0.0001;
if (Traj_Type == Hold_on), total_time = 1;end
if (Traj_Type == Traj_Slope), total_time = 2;end
if (Traj_Type == Traj_Pos_S), total_time = 4;end
if (Traj_Type == Traj_Vol_S), total_time = 7;end
time = 0:dt:total_time; % 时间序列
step = total_time/dt + 1; % 步数

%% 初始化保存仿真计算结果的数组，均为2行、step列矩阵，行数代表关节编号，列数为迭代步数
J_theta = zeros(2,step); % 记录关节实际转角数组，输出图形的纵坐标轴
J_omega = zeros(2,step); % 记录关节实际转速，输出图形的纵坐标轴
J_epsilon = zeros(2,step); % 记录关节实际加速度，输出图形的纵坐标轴
tmp = zeros(2,step);
%% 初始化仿真过程的中间变量，均为2行、1列，行数代表关节编号，这里根据题义，假定两关节均从0°位置零速启动
J_theta_pre = zeros(2,1); % 迭代计算前，当前关节实际转角
J_theta_aft = zeros(2,1); % 迭代计算后，当前关节实际转角
J_omega_pre = zeros(2,1); % 迭代计算前，当前关节实际转速
J_omega_aft = zeros(2,1); % 迭代计算后，当前关节实际转速
J_epsilon_pre = zeros(2,1); % 迭代计算后，当前关节实际加速度
J_epsilon_aft = zeros(2,1); % 迭代计算后，当前关节实际加速度
M_theta_pre = zeros(2,1); % 迭代计算前，当前电机实际转角
M_theta_aft = zeros(2,1); % 迭代计算后，当前电机实际转角
M_omega_pre = zeros(2,1); % 迭代计算前，当前电机实际转速
M_omega_aft = zeros(2,1); % 迭代计算后，当前电机实际转速
M_epsilon_pre = zeros(2,1); % 迭代计算后，当前电机实际加速度
M_epsilon_aft = zeros(2,1); % 迭代计算后，当前电机实际加速度

Tqm = zeros(2,1); % 电机控制力矩
Tqd = zeros(2,1); % 等效到电机转子的扰动力矩
Tqa = zeros(2,1); % 作用于电机转子的等效力矩
Tq_f = zeros(2,1); % 当前理论非线性力矩
Uc = zeros(2,1); % 控制电压
Ud = zeros(2,1); % 反应干扰力矩影响的放大器输入端等效扰动电压
Uc_d = zeros(2,1); % 作用于放大器的等效电压
Ue = zeros(2,1); % 电机感应电动势
ia_now = zeros(2,1); % 当前电枢电流
q_E = zeros(2,1); % 真实位置与期望位置误差
qd_E = zeros(2,1); % 真实速度与输入速度误差
global qd_E_integral; % 定义误差积分为全局变量，使PID控制器调用时可持续累加
qd_E_integral = zeros(2,1);

%% 设置加速度、速度和位置期望值
%% 位置保持
if (Traj_Type == Hold_on)
    J_theta_set = zeros(2,step);% 设置所有插补点处的关节期望转角，置为零
    J_omega_set = zeros(2,step);% 设置所有插补点处的关节期望速度，置为零
    J_epsilon_set = zeros(2,step);% 设置所有插补点处的关节期望速度，置为零
end
%% 位置斜坡轨迹
if (Traj_Type == Traj_Slope)
    [J_epsilon_set(1,:),J_omega_set(1,:),J_theta_set(1,:)] = Slope_traj(total_time,dt,pi/4,pi/4);
    [J_epsilon_set(2,:),J_omega_set(2,:),J_theta_set(2,:)] = Slope_traj(total_time,dt,pi/4,pi/4);
end
%% 位置S轨迹
if (Traj_Type == Traj_Pos_S)
    [J_epsilon_set(1,:), J_omega_set(1,:), J_theta_set(1,:)] = Position_S_traj(total_time,dt,pi/8);
    [J_epsilon_set(2,:), J_omega_set(2,:), J_theta_set(2,:)] = Position_S_traj(total_time,dt,pi/8);
end
%% 速度S轨迹
if (Traj_Type == Traj_Vol_S)
    [J_epsilon_set(1,:), J_omega_set(1,:), J_theta_set(1,:)] = Velocity_S_traj(total_time,dt,pi/20);
    [J_epsilon_set(2,:), J_omega_set(2,:), J_theta_set(2,:)] = Velocity_S_traj(total_time,dt,pi/20);
end

%% 主循环
for i = 1:step


    %% 获得关节空间期望值
    J_theta_d = J_theta_set(:,i); % 关节期望位置
    J_omega_d = J_omega_set(:,i); % 关节期望速度
    J_epsilon_d = J_epsilon_set(:,i); % 关节期望加速度 
   
    %% 获得驱动空间的控制期望值
    M_theta_d = N.* J_theta_d; % 电机期望位置
    M_omega_d = N .* J_omega_d; % 电机期望速度
    M_epsilon_d =N .* J_epsilon_d; % 电机期望加速度 

    %% 根据平面2R机器人非线性力矩模型和理论参数，计算当前理论扰动力矩，用于力矩前馈补偿
    Tq_f = P2R_Robot_Nonlinear_Inv_Dynamic(m_D,g,L_D,N,J_theta_d, J_omega_d, J_epsilon_d); % 根据期望状态值计算，小传动比会失稳
    
    tmp(:,i) = Tq_f;
%     Tq_f = P2R_Robot_Nonlinear_Inv_Dynamic(m_D,g,L_D,N,J_theta_pre, J_omega_pre, J_epsilon_pre); % 根据反馈的实际状态值计算，小传动比也不会失稳？？？
     %% 调用PID控制器，得到控制电压
    Uc = PID_Feedforward_Controller(M_theta_d, M_omega_d, M_epsilon_d, M_theta_pre, M_omega_pre, Kp, Kv, Tv, Kvff, Kaff, Ktff, Vol_Forward, Acc_Forward, Tq_Forward,Tq_f, dt);
    if (Uc(1,1) > 10),Uc(1,1) = 10; end % 控制电压上限
    if (Uc(2,1) > 10),Uc(2,1) = 10; end % 控制电压上限
    if (Uc(1,1) < -10),Uc(1,1) = -10; end % 控制电压上限
    if (Uc(2,1) < -10),Uc(2,1) = -10; end % 控制电压上限
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %% 更新电机角速度
    % 根据电流-力矩关系利用电机动力学模型计算电机输出转速，比电机通用模型更准确 
    if (Amp_Type == Amp_Voltage) % 速度模式电机的电流计算
        ia_now = (La/Ra)/(La/Ra + dt) .* ia_now + (dt/Ra)/(La/Ra + dt) .* (Ku .* Uc - Ue); % 电流迭代
        Ue = Ke .* M_omega_pre; % 该时刻反电动势
    end

    if(Amp_Type == Amp_Current) % 力矩模式电机的电流计算
        ia_now = Kg .* Uc; % 当前电枢电流
    end
    
    Tqm = Ka .* ia_now; % 该时刻控制力矩
    Tqm = [0;0];
    % 根据平面2R机器人非线性力矩模型和实际参数，计算实际非线性力矩Tq_d，用于模拟干扰力矩
    Tq_d = P2R_Robot_Nonlinear_Inv_Dynamic(m,g,L,N,J_theta_pre, J_omega_pre, J_epsilon_pre);
    
    Tqa = Tqm - Tq_d; % 作用于电机转子的等效合力矩
    
    %% 仿真模拟
    %% 根据电机动力学方程严格离散化形式计算角速度，然后计算位置和角加速度
%     M_omega_aft = (Mm_Average * inv(Bm))/(Mm_Average * inv(Bm) + dt .* eye(2))*M_omega_pre + (dt .* eye(2)*inv(Bm))* inv(Mm_Average * inv(Bm) + dt .* eye(2))*Tqa; % 根据电流-力矩关系利用电机动力学模型计算电机输出转速，比电机通用模型更准确 
%     M_theta_aft = M_theta_pre + M_omega_aft .* dt; % 电机实际角度
%     M_epsilon_aft = (M_omega_aft - M_omega_pre) ./ dt; % 电机实际角加速度
    
    %% 根据动力学方程迭代计算角加速度，然后计算速度和位置，与上述方法结果相同
    M_epsilon_aft = Mm_Average \ (Tqa - Bm * J_omega_pre);
    M_omega_aft = M_omega_pre + M_epsilon_aft .* dt;
    M_theta_aft = M_theta_pre + M_omega_aft .* dt;

    %% 更新关节状态值
    J_omega_aft = M_omega_aft ./ N; % 关节实际角速度
    J_theta_aft = M_theta_aft ./ N; % 关节实际角度
    J_epsilon_aft = M_epsilon_aft ./ N; % 关节实际角加速度
    
    J_theta(:,i) =  J_theta_aft; % 记录当前关节角度
    J_omega(:,i) =  J_omega_aft; % 记录当前关节角速度
    J_epsilon(:,i) =  J_epsilon_aft; % 记录当前关节角加速度
    
    %% 更新迭代前的电机和关节状态变量
    M_theta_pre = M_theta_aft; % 更新迭代前电机实际转角
    M_omega_pre = M_omega_aft; % 更新迭代前电机实际转速
    M_epsilon_pre = M_epsilon_aft; % 更新迭代前电机实际角加速度
    J_theta_pre = J_theta_aft; % 更新迭代前关节实际转角
    J_omega_pre = J_omega_aft; % 更新迭代前关节实际转速
    J_epsilon_pre = J_epsilon_aft; % 更新迭代前关节实际角加速度    
    
end

%% 绘图
%% 1#关节加速度曲线
figure(1);
yyaxis left;
% ylabel('ε(t)');
p = plot(time,J_epsilon(1,:),'k-',time,J_epsilon_set(1,:),'b--');
p(1).LineWidth=1.5;
p(2).LineWidth=1;
% if (Traj_Type == Traj_Pos_S), ylim([-3,3]);end
% if (Traj_Type == Traj_Vol_S), ylim([-5,5]);end
set(gca,'ycolor','k');
yyaxis right;
% ylabel('ε_e(t)');
p = plot(time, tmp(1,:), 'r-.');
p.LineWidth = 0.8;
if (Traj_Type == Traj_Pos_S), ylim([-2,4]);end
set(gca,'ycolor','k');
title(sprintf("1#关节加速度响应曲线"));
